// $Id: ao.c 405 2014-03-06 04:33:08Z alan.watson.f $

// This file is part of the control system for the Oaxaca instrument.

////////////////////////////////////////////////////////////////////////////////

#define _POSIX_C_SOURCE 200112L

#include <float.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#include <sys/time.h>

#include "oaxaca-config.h"
#include "oaxaca.h"
#include "ao.h"
#include "array.h"
#include "spots.h"
#include "minmax.h"

static double a0[OAXACA_NA_MAX];
static double a1[OAXACA_NA_MAX];
static double a2[OAXACA_NA_MAX];

static double da0[OAXACA_NA_MAX];
static double da1[OAXACA_NA_MAX];
static double da2[OAXACA_NA_MAX];

static double ds0[OAXACA_NS_MAX];
static double ds1[OAXACA_NS_MAX];
static double ds2[OAXACA_NS_MAX];

static double dt0[OAXACA_NS_MAX];
static double dt1[OAXACA_NS_MAX];
static double dt2[OAXACA_NS_MAX];

static int na = OAXACA_NA_MAX;

void
oaxaca_ao_clear(int index)
{
  volatile oaxaca_detector_t *detector = &oaxaca_shm->detector[index];

  for (int i = 0; i < OAXACA_NS_MAX; ++i) {
    detector->dsmean[i]  = 0.0;
    detector->dssigma[i] = 0.0;
  }

  for (int i = 0; i < OAXACA_NS_MAX; ++i)
    ds0[i] = ds1[i] = ds2[i] = 0.0;

  for (int i = 0; i < OAXACA_NS_MAX; ++i)
    dt0[i] = dt1[i] = dt2[i] = 0.0;

  for (int i = 0; i < OAXACA_NA_MAX; ++i)
    da0[i] = da1[i] = da2[i] = 0.0;

  for (int i = 0; i < OAXACA_NA_MAX; ++i)
    a0[i] = a1[i] = a2[i] = 0.0;
}

void
oaxaca_ao_frame_call_back(int index, array_short_t *frame)
{
  volatile oaxaca_detector_t *detector = &oaxaca_shm->detector[index];

  double ds[OAXACA_NS_MAX];

  int ns = detector->ns;

  double r[OAXACA_NS_MAX];
  for (int i = 0; i < ns; ++i)
    r[i] = detector->r[i];

  double s[OAXACA_NS_MAX];
  oaxaca_spots_refine(frame, ns, r, detector->searchradius, detector->refinementradius, s);
  
  for (int i = 0; i < ns; ++i)
    detector->s[i] = s[i];

  for (int i = 0; i < ns; ++i)
    ds[i] = s[i] - r[i];

  for (int i = 0; i < ns; ++i) {
    ds0[i] += 1.0;
    ds1[i] += ds[i];
    ds2[i] += ds[i] * ds[i];
  }

  if (index == OAXACA_DETECTOR_INDEX_WFS) {

    double dt[ns];
    for (int j = 0; j < ns; ++j) {
      dt[j] = ds[j];
      for (int i = 0; i < na; ++i)
        dt[j] += detector->i_matrix[i][j] * oaxaca_shm->wfc.a[i];
    }

    // Measure longitudinal displacements between spots in the same row or column with b = 4.
    int nt = 6;
    double ddt[nt];
    ddt[0] = dt[ 9 * 2 + 0] - dt[ 0 * 2 + 0];
    ddt[1] = dt[10 * 2 + 0] - dt[ 1 * 2 + 0];
    ddt[2] = dt[11 * 2 + 0] - dt[ 2 * 2 + 0];
    ddt[3] = dt[ 4 * 2 + 1] - dt[ 3 * 2 + 1];
    ddt[4] = dt[ 6 * 2 + 1] - dt[ 5 * 2 + 1];
    ddt[5] = dt[ 8 * 2 + 1] - dt[ 7 * 2 + 1];
    for (int i = 0; i < nt; ++i) {
      dt0[i] += 1.0;
      dt1[i] += ddt[i];
      dt2[i] += ddt[i] * ddt[i];
    }
  }

    
  double da[na];
  for (int i = 0; i < na; ++i) {
    da[i] = 0.0;
    for (int j = 0; j < ns; ++j)
      da[i] += detector->c_matrix[i][j] * ds[j];
    da[i] *= detector->g[i];
  }
  
  da[0] = max(-0.1, min(+0.1, da[0]));
  da[1] = max(-0.1, min(+0.1, da[1]));
  
  for (int i = 0; i < na; ++i) {
    double a = oaxaca_shm->wfc.a[i];
    a -= da[i];
    if (a != a)
      a = 0.0;
    else if (a < -1.0)
      a = -1.0;
    else if (a > +1.0)
      a = +1.0;
    oaxaca_shm->wfc.a[i] = a;
  }
  
  oaxaca_shm->wfc.timestamp = detector->timestamp;

#if 0

  // This code is an experiment to determine the WFC response delay.
  
  static FILE *testfp;
  static double sref[OAXACA_NS_MAX];
  if (detector->frame == 50) {
    testfp = fopen("/tmp/test.csv", "w");
  }
  if (50 <= detector->frame && detector->frame <= 150) {
    fprintf(testfp, "%lu", detector->frame);
    for (int i = 0; i < ns; ++i)
      fprintf(testfp, ",%.2f", s[i] - sref[i]);
    fprintf(testfp, "\n");
    for (int i = 0; i < ns; i += 2)
      oaxaca_info("test: frame %lu spot %d (%+.2f,%+.2f).", detector->frame, i / 2, s[i] - sref[i], s[i + 1] - sref[i + 1]);
  }
  if (detector->frame == 150)
    fclose(testfp);
  for (int i = 0; i < ns; ++i)
    sref[i] = s[i];

  if (detector->frame == 0) {
    oaxaca_info("test: setting WFC to zero.");
    for (int i = 0; i < na; ++i)
      oaxaca_shm->wfc.a[i] = 0;
    oaxaca_shm->wfc.command = 't';
  } else if (detector->frame == 100) {
    oaxaca_info("test: moving WFC.");
    //oaxaca_shm->wfc.a[0] = -0.1;		// TM
    oaxaca_shm->wfc.a[1] = 1.0;		// DM
    oaxaca_shm->wfc.command = 't';
  }

 #else
 
   // This code is for normal correction.
 
  oaxaca_shm->wfc.command = 't';
 
 #endif

  struct timeval tv;
  gettimeofday(&tv, NULL);
  double timestamp = tv.tv_sec + tv.tv_usec * 1e-6;
  detector->totaldelay += timestamp - detector->timestamp;
    
  for (int i = 0; i < na; ++i) {
    da0[i] += 1.0;
    da1[i] += da[i];
    da2[i] += da[i] * da[i];
  }

  for (int i = 0; i < na; ++i) {
    double a = oaxaca_shm->wfc.a[i];
    a0[i] += 1.0;
    a1[i] += a;
    a2[i] += a * a;
  }

}

void
oaxaca_ao_integration_call_back(int index, array_short_t *mean)
{
  volatile oaxaca_detector_t *detector = &oaxaca_shm->detector[index];

  int ns = detector->ns;

  {
    double sumsq = 0.0;
    for (int i = 0; i < ns; ++i) {
      double mean = ds1[i] / ds0[i];
      double variance = (ds0[i] * ds2[i] - ds1[i] * ds1[i]) / ds0[i] / (ds0[i] - 1.0);
      double sigma = (variance >= 0.0) ? sqrt(variance) : 0.0;
      detector->dsmean[i]  = mean;
      detector->dssigma[i] = sigma;
      //oaxaca_info("dsmean[%d] = %+.2f dssigma[%d] = %.2f", i, mean, i, sigma);
      sumsq += variance;
    }
    double dssigma = sqrt(sumsq / ns);
    oaxaca_info("dssigma = %.3f pixels (raw)", dssigma);
    double dssigmanoise = 57.294 * pow(detector->eshm, -0.8748);
    oaxaca_info("dssigma = %.3f pixels (noise)", dssigmanoise);
    double dssigmacorrected = (dssigmanoise > dssigma) ? 0.0 : sqrt(dssigma * dssigma - dssigmanoise * dssigmanoise);
    oaxaca_info("dssigma = %.3f pixels (corrected)", dssigmacorrected);
    double pi = 4.0 * atan(1.0);
    double scale = 0.429;
    double fwhm = 2.0 * sqrt(2.0 * log(2.0)) * dssigmacorrected * scale;
    oaxaca_info("equivalent FWHM is %.2f arcsec.", fwhm);
  }
  
  if (index == OAXACA_DETECTOR_INDEX_WFS) {

    int nt = 6;
    double sumsq = 0.0;
    for (int i = 0; i < nt; ++i) {
      //double mean = dt1[i] / dt0[i];
      double variance = (dt0[i] * dt2[i] - dt1[i] * dt1[i]) / dt0[i] / (dt0[i] - 1.0);
      //double sigma = (variance >= 0.0) ? sqrt(variance) : 0.0;
      //oaxaca_info("dtmean[%d] = %+.2f dtsigma[%d] = %.2f", i, mean, i, sigma);
      sumsq += variance;
    }
    double dtsigma = sqrt(sumsq / nt);
    oaxaca_info("dtsigma = %.3f pixels (raw)", dtsigma);
    
    double dtsigmanoise = sqrt(2) * 57.294 * pow(detector->eshm, -0.8748);
    oaxaca_info("dtsigma = %.3f pixels (noise)", dtsigmanoise);
    
    double dtsigmacorrected = (dtsigmanoise > dtsigma) ? 0.0 : sqrt(dtsigma * dtsigma - dtsigmanoise * dtsigmanoise);
    oaxaca_info("dtsigma = %.3f pixels (corrected)", dtsigmacorrected);
    detector->dtsigma = dtsigmacorrected;
  
    double D = 0.13;
    double lambda = 0.75e-6;
    double b = 4.0;
    double K = 0.364 * (1.0 - 0.532 * pow(b, -1.0 / 3.0) - 0.024 * pow(b, - 7.0 / 3.0));
    oaxaca_info("K is %.3f.", K);

    double pi = 4.0 * atan(1.0);
    double arcsec = pi / (180.0 * 60.0 * 60.0);
    double scale = 0.429;

    double sigmad = dtsigmacorrected * scale * arcsec;
    
    double r0 = pow(K, 0.6) * pow(lambda / sigmad, 1.2) / pow (D, 0.2);
    oaxaca_info("r0 is %.1f cm at %.0f nm.", r0 * 100, lambda * 1e9);
    double fwhm = 0.98 * lambda / r0;
    oaxaca_info("seeing FWHM is %.2f arcsec at %.0f nm.", fwhm / arcsec, lambda * 1e9);

  }

  for (int i = 0; i < na; ++i) {
    double mean = a1[i] / a0[i];
    double variance = (a0[i] * a2[i] - a1[i] * a1[i]) / a0[i] / (a0[i] - 1);
    double sigma = (variance >= 0.0) ? sqrt(variance) : 0.0;
    oaxaca_shm->wfc.amean[i]  = mean;
    oaxaca_shm->wfc.asigma[i] = sigma;
  }
 
  for (int i = 0; i < na; ++i) {
    double mean = da1[i] / da0[i];
    double variance = (da0[i] * da2[i] - da1[i] * da1[i]) / da0[i] / (da0[i] - 1);
    double sigma = (variance >= 0.0) ? sqrt(variance) : 0.0;
    oaxaca_shm->wfc.damean[i]  = mean;
    oaxaca_shm->wfc.dasigma[i] = sigma;
  }
 
  if (
      detector->integration % 2 == 0 &&
      detector->g[2] > 0 && fabs(oaxaca_shm->wfc.amean[2]) > 0.2
    ) {
    int foffset = oaxaca_shm->wfc.amean[2] > 0 ? +1 : -1;
    FILE *fp = fopen("/tmp/telescope", "w");
    if (fp != NULL) {
      fprintf(fp, "CF%+d;\n", foffset);
      fclose(fp);
    }
    oaxaca_info("moving telescope focus %+d.", foffset);
  } else if (
      detector->integration % 2 == 1 &&
      detector->g[0] > 0 && 
      detector->g[1] > 0 && 
      (fabs(oaxaca_shm->wfc.amean[0]) > 0.2 || fabs(oaxaca_shm->wfc.amean[1]) > 0.2)
    ) {
    int eoffset;
    int noffset;
    if (oaxaca_shm->wfc.amean[0] > +0.2)
      eoffset = +1;
    else if (oaxaca_shm->wfc.amean[0] < -0.2)
      eoffset = -1;
    else
      eoffset = 0;
    if (oaxaca_shm->wfc.amean[1] > +0.2)
      noffset = -1;
    else if (oaxaca_shm->wfc.amean[1] < -0.2)
      noffset = +1;
    else
      noffset = 0;
    oaxaca_info("moving telescope %+d arcsec east and %+d arcsec north.", eoffset, noffset);
    FILE *fp = fopen("/tmp/telescope", "w");
    if (fp != NULL) {
      fprintf(fp, "MF%+04d%+04d;\n", -eoffset, noffset);
      fclose(fp);
    }
  }

  for (int i = 0; i < OAXACA_NS_MAX; ++i)
    ds0[i] = ds1[i] = ds2[i] = 0.0;
  for (int i = 0; i < OAXACA_NS_MAX; ++i)
    dt0[i] = dt1[i] = dt2[i] = 0.0;
  for (int i = 0; i < OAXACA_NA_MAX; ++i)
    da0[i] = da1[i] = da2[i] = 0.0;
  for (int i = 0; i < OAXACA_NA_MAX; ++i)
    a0[i] = a1[i] = a2[i] = 0.0;
}
